/*
 *  integrator.cpp
 *  EPPI-0.0
 *
 *  Created by Markus Lutz on 21/11/09.
 *  Modified by JCGO
 *	Modified by Sergio Conde
 *
 */

#include "integrator.h"
#include "simulation.h"
#include <cmath>

Integrator::Integrator()
:	type(),
	beta(0.25),
	gamma(0.5),
	alpha_f(),
	alpha_m(),
	rho_inf(0.0),
	partial_dq(0),
	partial_ddq(0),
	h(0.1),
	ddq_n_1_const(),
	dq_n_1_const(),
	pq(),
	pdq(),
	pddq(),
	pqConv(),
	pdqConv(),
	parentSim(),
	q_n(),
	q_n1(),
	q_n2(),
	q_n3(),
	q_n4(),
	dq_n(),
	dq_n1(),
	dq_n2(),
	dq_n3(),
	dq_n4(),
	ddq_n(),
	ddq_n1(),
	ddq_n2(),
	ddq_n3(),
	ddq_n4()
{
}

// ***

Integrator::Integrator(string ty, double stepsize, Simulation *pSim, map<string, double, less<string> > & intOptions)
:	type(ty),
	beta(0.25),
	gamma(0.5),
	alpha_f(),
	alpha_m(),
	rho_inf(0.0),
	partial_dq(0),
	partial_ddq(0),
	h(stepsize),
	ddq_n_1_const(),
	dq_n_1_const(),
	pq(),
	pdq(),
	pddq(),
	pqConv(),
	pdqConv(),
	parentSim(pSim),
	q_n(),
	q_n1(),
	q_n2(),
	q_n3(),
	q_n4(),
	dq_n(),
	dq_n1(),
	dq_n2(),
	dq_n3(),
	dq_n4(),
	ddq_n(),
	ddq_n1(),
	ddq_n2(),
	ddq_n3(),
	ddq_n4()
{
	/*
	type = ty;		// integrator type
	h = stepsize;		

	parentSim = pSim;	// pointer to parent Simulation
	*/
	
	pqConv = & parentSim->lProcess->qConv;	// get pointer to coverged position vector in process
	pdqConv = & parentSim->lProcess->dqConv;	// get pointer to coverged velocity vector in process

	pq =  & parentSim->lProcess->q;		// get pointer to position vector in process
	pdq = & parentSim->lProcess->dq;	// get pointer to velocity vector in process
	pddq = & parentSim->lProcess->ddq;	// get pointer to acceleration vector in process

	beta = intOptions["beta"];
	gamma = intOptions["gamma"];
	rho_inf = intOptions["rho_inf"];
	
	if (!type.compare("alphagen"))
	{
		alpha_m = (2.0 * rho_inf - 1.0) / (rho_inf + 1.0);
		alpha_f = rho_inf / (rho_inf + 1);
		gamma = 0.5 + alpha_f - alpha_m;
		beta = 0.25 * (gamma + 0.5) * (gamma + 0.5);

		cout << "alpha_m: " << alpha_m << endl;
		cout << "alpha_f: " << alpha_f << endl;
		cout << "gamma: " << gamma << endl;
		cout << "beta: " << beta << endl;
	}

	setPartials(h,type);	// calculate the partial derivative of df/q_n_1 and df/dq_n_1

	// set size of vectors
	q_n.resize(pq->size());
	q_n1.resize(pq->size());
	q_n2.resize(pq->size());
	q_n3.resize(pq->size());
	q_n4.resize(pq->size());
	dq_n.resize(pq->size());
	dq_n1.resize(pq->size());
	dq_n2.resize(pq->size());
	dq_n3.resize(pq->size());
	dq_n4.resize(pq->size());
	ddq_n.resize(pq->size());
	ddq_n1.resize(pq->size());
	ddq_n2.resize(pq->size());
	ddq_n3.resize(pq->size());
	ddq_n4.resize(pq->size());

	if  (!type.compare("alphagen")) a.resize(pq->size());
	dq_n_1_const.resize(pq->size());
	ddq_n_1_const.resize(pq->size());	
}

// ***

void Integrator::integrate(double time, int timeFlag, int numStep)
{
	// implicit integrators

	// Alpha generalized
	if  (!type.compare("alphagen"))
	{
		if (numStep == 0 && timeFlag == 1)
		{
			// initial value a
			a = *pddq;
		}

		/*
		if (numStep > 0 && timeFlag == 1)
		{
			for(int i = 0; i < a.size() ; ++i)
				a[i] = a[i] + (1 - alpha_f)/(1 - alpha_m) * (*pddq)[i];
		}*/

		alphaGen(time, timeFlag);
	}

	// Newmark
	if  (!type.compare("newmark"))
	{ 
		newmark(time, timeFlag);
	}

	// midpoint
	else if  (!type.compare("midpoint"))
	{
	    midpoint(time, timeFlag);
	}
	
	//Consistent
	else if (!type.compare("consistent"))
	{
       consistent(time, timeFlag);
    }
	
	// BDF family
	// bdf-1
	else if (!type.compare("bdf-1" ))
	{
		bdf_1(time,timeFlag);
	}

	// bdf-2
	else if (!type.compare("bdf-2" ))
	{
		if (numStep > 1) bdf_2(time,timeFlag);
		else if (numStep == 1)	
		{
			setPartials(h,type);
			bdf_2(time,timeFlag);
		}
		else 
		{	
			setPartials(h,"bdf-1");
			bdf_1(time,timeFlag);
		}
	}

	// bdf-3
	else if (!type.compare("bdf-3" ))
	{
		if (numStep > 2)	bdf_3(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"bdf-1");
					bdf_1(time,timeFlag);	
					break;	
				case 1: 
					setPartials(h,"bdf-2");	
					bdf_2(time,timeFlag);
					break;	
				case 2: 
					setPartials(h,"bdf-3");	
					bdf_3(time,timeFlag);
					break;			
				default: cout << "\n Something strange happening in integrator bdf - 3";
			}
		}
	}

	// bdf-4
	else if (!type.compare("bdf-4" ))
	{
		if (numStep > 3)	bdf_4(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"bdf-1");	
					bdf_1(time,timeFlag);
					break;	
				case 1: 
					setPartials(h,"bdf-2");
					bdf_2(time,timeFlag);	
					break;	
				case 2: 
					setPartials(h,"bdf-3");	
					bdf_3(time,timeFlag);
					break;	
				case 3: 
					setPartials(h,"bdf-4");	
					bdf_4(time,timeFlag);
					break;			
				default: cout << "\n Something strange happening in integrator bdf - 4";
			}
		}
	}

	// bdf-5
	else if (!type.compare("bdf-5" ))
	{
		if (numStep > 4)	bdf_5(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"bdf-1");	
					bdf_1(time,timeFlag);
					break;	
				case 1: 
					setPartials(h,"bdf-2");	
					bdf_2(time,timeFlag);
					break;	
				case 2: 
					setPartials(h,"bdf-3");	
					bdf_3(time,timeFlag);
					break;	
				case 3: 
					setPartials(h,"bdf-4");	
					bdf_4(time,timeFlag);
					break;	
				case 4: 
					setPartials(h,"bdf-5");
					bdf_5(time,timeFlag);	
					break;			
				default: cout << "\n Something strange happening in integrator bdf - 5";
			}
		}
	}

// Adams - Moulton family

	// am-1
	else if (!type.compare("am-1" ))
	{
		am_1(time,timeFlag);
	}

	// am-2
	else if (!type.compare("am-2" ))
	{
		if (numStep > 1) am_2(time,timeFlag);
		else if (numStep == 1)	
		{
			setPartials(h,type);
			am_2(time,timeFlag);
		}
		else 
		{	
			setPartials(h,"am-1");
			am_1(time,timeFlag);
		}
	}

	// am-3
	else if (!type.compare("am-3" ))
	{
		if (numStep > 2)	am_3(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"am-1");
					am_1(time,timeFlag);	
					break;	
				case 1: 
					setPartials(h,"am-2");	
					am_2(time,timeFlag);
					break;	
				case 2: 
					setPartials(h,"am-3");
					am_3(time,timeFlag);	
					break;				
				default: cout << "\n Something strange happening in integrator am - 3";
			}
		}
	}

	// am-4
	else if (!type.compare("am-4" ))
	{
		if (numStep > 3)	am_4(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"am-1");	
					am_1(time,timeFlag);
					break;	
				case 1: 
					setPartials(h,"am-2");	
					am_2(time,timeFlag);
					break;	
				case 2: 
					setPartials(h,"am-3");	
					am_3(time,timeFlag);
					break;	
				case 3: 
					setPartials(h,"am-4");
					am_4(time,timeFlag);	
					break;				
				default: cout << "\n Something strange happening in integrator am - 4";
			}
		}
	}

	// am-5
	else if (!type.compare("am-5" ))
	{
		if (numStep > 4)	am_5(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					setPartials(h,"am-1");
					am_1(time,timeFlag);	
					break;	
				case 1: 
					setPartials(h,"am-2");
					am_2(time,timeFlag);	
					break;	
				case 2: 
					setPartials(h,"am-3");
					am_3(time,timeFlag);	
					break;	
				case 3: 
					setPartials(h,"am-4");
					am_4(time,timeFlag);	
					break;	
				case 4: 
					setPartials(h,"am-5");
					am_5(time,timeFlag);	
					break;			
				default: cout << "\n Something strange happening in integrator am - 5";
			}
		}
	}


	// explicit integrators

// Adam-Bashford family

	// ab-1
	else if (!type.compare("ab-1" ))
	{
		ab_1(time,timeFlag);
	}

	// ab-2
	else if (!type.compare("ab-2" ))
	{
		if (numStep > 1) ab_2(time,timeFlag);
		else if (numStep == 1)	
		{
			//setPartials(h,type);
			ab_2(time,timeFlag);
		}
		else 
		{	
			//setPartials(h,"ab-1");
			ab_1(time,timeFlag);
		}
	}

	// ab-3
	else if (!type.compare("ab-3" ))
	{
		if (numStep > 2) ab_3(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					ab_1(time,timeFlag);
					//setPartials(h,"ab-1");	
					break;	
				case 1: 
					ab_2(time,timeFlag);
					//setPartials(h,"ab-2");	
					break;	
				case 2: 
					ab_3(time,timeFlag);
					//setPartials(h,"ab-3");	
					break;			
				default: cout << "\n Something strange happening in integrator ab - 3";
			}
		}
	}

	// ab-4
	else if (!type.compare("ab-4" ))
	{
		if (numStep > 3) ab_4(time,timeFlag);
		else 
		{
			switch (numStep)
			{
				case 0: 
					ab_1(time,timeFlag);
					//setPartials(h,"ab-1");	
					break;	
				case 1: 
					ab_2(time,timeFlag);
					//setPartials(h,"ab-2");	
					break;	
				case 2: 
					ab_3(time,timeFlag);
					//setPartials(h,"ab-3");	
					break;
				case 3: 
					ab_4(time,timeFlag);
					//setPartials(h,"ab-3");	
					break;
				default: cout << "\n Something strange happening in integrator ab - 4";
			}
		}
	}

// Runge-Kutta family

	else if (!type.compare("RK-1"))
	{
		rk_1(time);
	}
	else if (!type.compare("RK-2"))
	{
		rk_2(time);
	}
	else if (!type.compare("RK-3"))
	{
		rk_3(time);
	}
	else if (!type.compare("RK-4"))
	{
		rk_4(time);
	}
// Central differences
	else if (!type.compare("cd"))
	{
		cd(time, timeFlag);
	}
	else
	{
		cout << "\t**ERROR: " << type << " integrator is not available, a bdf-1 integrator will be employed instead." << endl;
		type = "bdf-1";

		setPartials(h,"bdf-1");
		bdf_1(time,timeFlag);
	}

}

// **


// ****************************************************************
// here start the different integrators
//
// alpha - generalized method
// newmark
// midpoint
// consistent
// bdf-1
// bdf-2
// bdf-3
// bdf-4
// bdf-5
// rk-1
// rk-2
// rk-4
// ab-1
// ab-2
// ab-3
// ab-4


void Integrator::alphaGen(double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary
	{
		q_n = *pq;
		dq_n = *pdq;

		for (i=0; i<q_n.size(); ++i) 
		{
			q_n4[i] = (*pq)[i] + h * (*pdq)[i] + h*h * (0.5 - beta) * a[i];
			ddq_n_1_const[i] = alpha_m * a[i] - alpha_f * (*pddq)[i];		// calcute part of the acceleration which keeps constant during the iteration process
			dq_n_1_const[i] = (*pdq)[i] + (1 - gamma) * h * a[i];			// calcute part of the acceleration which keeps constant during the iteration process
		}
	}

	for (i=0; i<q_n.size(); ++i) 
	{
		a[i] = 1.0 / (h*h*beta) * ((*pq)[i] - q_n4[i]);
		(*pdq)[i] = dq_n_1_const[i] + gamma * h * a[i];		
		(*pddq)[i] = 1.0/(1.0 - alpha_f) * ((1.0 - alpha_m) * a[i] + ddq_n_1_const[i]);			
	}
}

// ****

void Integrator::newmark(double time, int timeFlag)
{	
	int i;
/*
	cout << "Old acceleration is " << endl;
	printSTLVector((*pddq));
*/
	if(timeFlag) // check if new calculation of the constant part is necessary
	{
		for (i=0; i<(*pq).size(); ++i) 
		{
			ddq_n_1_const[i] = (*pq)[i] + h * (*pdq)[i] + (0.5 - beta) * h * h * (*pddq)[i];	// calcute part of the acceleration which keeps constant during the iteration process
			dq_n_1_const[i] = (*pdq)[i] + (1.0 - gamma) * h * (*pddq)[i];			// calcute part of the acceleration which keeps constant during the iteration process
		}
	}
/*
	cout << "For position is " << endl;
	printSTLVector((*pq));
	cout << "Old velocity is " << endl;
	printSTLVector((*pdq));	
	cout << "Old acceleration is " << endl;
	printSTLVector((*pddq));
*/	


	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pddq)[i] = 1.0 / (beta * h * h) * ( (*pq)[i] - ddq_n_1_const[i] );		// new acceleration
		(*pdq)[i] = dq_n_1_const[i] + (gamma/(beta*h))*( (*pq)[i] - ddq_n_1_const[i] );	// new velocity
	}

	
	// set to 0.0 the acceleration of dofs that do not contribute to inertia
	for (i=0; i<parentSim->lProcess->numNoInertia.size(); ++i) 
	{
		(*pddq)[ parentSim->lProcess->numNoInertia[i] ] = 0.0;
	}
/*	
	cout << "gamma = " << gamma << endl;
	cout << "beta = " << beta << endl;
	cout << "New velocity is " << endl;
	printSTLVector((*pdq));

	cout << "New acceleration is " << endl;
	printSTLVector((*pddq));
*/
}

// **

void Integrator::midpoint(double time, int timeFlag)
{
	int i;
	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pdq)[i] = (2.0/h)*( (*pq)[i] - (*pqConv)[i] ) - (*pdqConv)[i];
	}	
}

// ***

void Integrator::consistent(double time, int timeFlag)
{
	int i;
	for (i=0; i<(*pq).size(); ++i)
	{
		(*pdq)[i] = (2.0/h)*( (*pq)[i] - (*pqConv)[i] ) - (*pdqConv)[i];
	}
}

// ***

void Integrator::bdf_1(	double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary
	{
		// update history 

		// position vectors
		q_n = *pq;

		// velocity vectors
		dq_n = *pdq;
		// calculate constant part
		dq_n_1_const = q_n;			// calculate constant part 
		ddq_n_1_const = dq_n;			// calculate constant part
	}

	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pdq)[i] = 1.0 / (h * 1.0) * ( (*pq)[i] - dq_n_1_const[i] );	// calculate velocity
		(*pddq)[i] = 1.0 / (h * 1.0) * ( (*pdq)[i] - ddq_n_1_const[i] );	// calculate acceleration
	}
}

// ***

void Integrator::bdf_2(	double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary and update the last steps
	{
		// update history 

		// position vectors
		q_n1 = q_n;
		q_n = *pq;

		// velocity vectors
		dq_n1 = dq_n;
		dq_n = *pdq;

		// calculate constant part
		for (i=0; i<(*pq).size(); ++i) 
		{
			dq_n_1_const[i] =  (q_n[i] *  (4.0/3.0) -  q_n1[i] * (1.0/3.0));		// calculate constant part 
			ddq_n_1_const[i] = (dq_n[i] * (4.0/3.0) - dq_n1[i] * (1.0/3.0));			// calculate constant part
		}
	}
		 
	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pdq)[i] = 1.0 / (h * (2.0/3.0)) * ( (*pq)[i] - dq_n_1_const[i] );		// calculate velocity
		(*pddq)[i] = 1.0 / (h * (2.0/3.0)) * ( (*pdq)[i] - ddq_n_1_const[i] );		// calculate acceleration/
	}
}

// ***

void Integrator::bdf_3(	double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary and update the last steps
	{
		// update history 
		// position vectors
		q_n2 = q_n1;
		q_n1 = q_n;
		q_n = *pq;

		// velocity vectors
		dq_n2 = dq_n1;
		dq_n1 = dq_n;
		dq_n = *pdq;
		// calculate constant part
		for (i=0; i<q_n.size(); ++i) 
		{
			dq_n_1_const[i]  =   q_n[i] * (18.0/11.0) +  q_n1[i] * (-9.0/11.0) +  q_n2[i] * (2.0/11.0);		// calculate constant part 
			ddq_n_1_const[i] = dq_n[i] * (18.0/11.0) + dq_n1[i] * (-9.0/11.0) + dq_n2[i] * (2.0/11.0);		// calculate constant part
		}
	}
	for (i=0; i<q_n.size(); ++i) 
	{
		(*pdq)[i] = 1.0 / (h * 6.0/11.0) * ( (*pq)[i] - dq_n_1_const[i] );		// calculate velocity
		(*pddq)[i] = 1.0 / (h * 6.0/11.0) * ( (*pdq)[i] - ddq_n_1_const[i] );	// calculate acceleration/
	}
}

// ***

void Integrator::bdf_4(	double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary and update the last steps
	{
		// update history 
		// position vectors
		q_n3 = q_n2;
		q_n2 = q_n1;
		q_n1 = q_n;
		q_n = *pq;

		// velocity vectors
		dq_n3 = dq_n2;
		dq_n2 = dq_n1;
		dq_n1 = dq_n;
		dq_n = *pdq;

		// calculate constant part
		for (i=0; i<q_n.size(); ++i) 
		{
			dq_n_1_const[i] =   q_n[i] * (48.0/25.0) +  q_n1[i] * (-36.0/25.0) +  q_n2[i] * (16.0/25.0) +  q_n3[i] * (-3.0/25.0);			// calculate constant part 
			ddq_n_1_const[i] =  dq_n[i] * (48.0/25.0) +  dq_n1[i] * (-36.0/25.0) +  dq_n2[i] * (16.0/25.0) +  dq_n3[i] * (-3.0/25.0);		// calculate constant part
		}
	}
	for (i=0; i<q_n.size(); ++i) 
	{
		(*pdq)[i] = 1.0 / (h * 12.0/25.0) * ( (*pq)[i] - dq_n_1_const[i] );		// calculate velocity
		(*pddq)[i] = 1.0 / (h * 12.0/25.0) * ( (*pdq)[i] - ddq_n_1_const[i] );		// calculate acceleration/
	}
}

// ***

void Integrator::bdf_5(	double time, int timeFlag)
{
	int i;
	if(timeFlag) // check if new calculation of the constant part is necessary and update the last steps
	{
		// update history 

		// position vectors
		q_n4 = q_n3;
		q_n3 = q_n2;
		q_n2 = q_n1;
		q_n1 = q_n;
		q_n = *pq;

		// velocity vectors
		dq_n4 = dq_n3;
		dq_n3 = dq_n2;
		dq_n2 = dq_n1;
		dq_n1 = dq_n;
		dq_n = *pdq;

		// calculate constant part
		for (i=0; i<q_n.size(); ++i) 
		{
			dq_n_1_const[i] =   q_n[i] * (300.0/137.0) +  q_n1[i] * (-300.0/137.0) +  q_n2[i] * (200.0/137.0) +  q_n3[i] * (-75.0/137.0)	+  q_n4[i] * (12.0/137.0);		// calculate constant part 
			ddq_n_1_const[i] =  dq_n[i] * (300.0/137.0) +  dq_n1[i] * (-300.0/137.0) +  dq_n2[i] * (200.0/137.0) + dq_n3[i] * (-75.0/137.0) +  dq_n4[i] * (12.0/137.0);		// calculate constant part
		}
	}
	for (i=0; i<q_n.size(); ++i) 
	{
		(*pdq)[i] = 1.0 / (h * 60.0/137.0) * ( (*pq)[i] - dq_n_1_const[i] );		// calculate velocity
		(*pddq)[i] = 1.0 / (h * 60.0/137.0) * ( (*pdq)[i] - ddq_n_1_const[i] );	// calculate acceleration/
	}
}

// ***

void Integrator::rk_1(	double time)
{			
	int i;
	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pq)[i]  = (*pq)[i]  + h * (*pdq)[i];	 // new position at n+1
		(*pdq)[i] = (*pdq)[i] + h * (*pddq)[i]; // new velocity at n+1
	}

//	parentSim->lProcess->myEvaluationExplicit(time+h); // new acceleration at at n+1
}


// ***

void Integrator::rk_2(double time)
{
		

/*
			// save vector of the n-th step			
			lmx::Vector<double> save_qn   =  *pq;
			lmx::Vector<double> save_dqn  =  *pdq;
			lmx::Vector<double> save_ddqn =  *pddq;

	
			// first step
			lmx::Vector<double> q_n_s1(save_qn.size());
			lmx::Vector<double> dq_n_s1(save_qn.size());
			lmx::Vector<double> ddq_n_s1(save_qn.size());
			

			// calculate solution of first step 
			q_n_s1 = save_qn   + h/2.0 * save_dqn;
			dq_n_s1 = save_dqn + h/2.0 * save_ddqn;


			*pq  = q_n_s1;
			*pdq = dq_n_s1;

			parentSim->lProcess->myEvaluationExplicit(time+h/2);

					
			// calculate solution of second step 
			*pq = save_qn   + h * dq_n_s1;
			*pdq = save_dqn + h * *pddq; 

			parentSim->lProcess->myEvaluationExplicit(time+h);			
*/		
}

// ***

void Integrator::rk_3(double time) // TO BE DONE....
{
}

// ***

void Integrator::rk_4(	double time)
{
		
/*
			// save vector of the n-th step			
			lmx::Vector<double> save_qn   =  *pq;       // position at n
			lmx::Vector<double> save_dqn  =  *pdq;	     // velocity at n
	
			parentSim->lProcess->myEvaluationExplicit(time);  // calculate acceleration at n	
			lmx::Vector<double> save_ddqn =  *pddq;     // acceleration at n // used?




// ************************************ 	
// first step
// q_s_n_05 = q_n + h/2 * f(t_n,q_n)


			lmx::Vector<double> dq_n_s1(save_qn.size());   //used
			lmx::Vector<double> ddq_n_s1(save_qn.size());  //used
			

			// save f(t_n,q_n)
			dq_n_s1     = *pdq; 	// save f(t_n,q_n)
			ddq_n_s1    = *pddq;   // save f(t_n,q_n)

			// calculate solution of first step and save it in process
			*pq = save_qn   + h/2.0 * dq_n_s1;
			*pdq = save_dqn + h/2.0 * ddq_n_s1;
	
			parentSim->lProcess->myEvaluationExplicit(time+h/2); 	// calculate new acceleration at t = t_n + h/2



// *********************************		
// second step
// q_ss_n_05 = q_n + h/2 * f(t_n_05,q_s_n_05)	
		
			
			lmx::Vector<double> dq_n_s2(save_qn.size());	//used
			lmx::Vector<double> ddq_n_s2(save_qn.size());	//used
	
			// f(t_n_05,q_s_n_05)
			dq_n_s2     = *pdq; 	// save f(t_n_05,q_s_n_05)
			ddq_n_s2    = *pddq;   // save f(t_n_05,q_s_n_05)
			
			// calculate solution of second step and save it in process
			*pq =  save_qn   + h/2.0 * dq_n_s2;		
			*pdq = save_dqn  + h/2.0 * ddq_n_s2 ;
			parentSim->lProcess->myEvaluationExplicit(time+h);	// calculate new acceleration for next step at t = t_n + h



// *********************************		
// third step
// q_s_n_1 = q_n + h/2 * f(t_n_1,q_ss_n_05)			

			
			lmx::Vector<double> dq_n_s3(save_qn.size());	//used
			lmx::Vector<double> ddq_n_s3(save_qn.size());	//used
	
			// f(t_n_1,q_s_n_05)
			dq_n_s3     = *pdq; 	// save f(t_n_1,q_ss_n_05)
			ddq_n_s3    = *pddq;   // save f(t_n_1,q_ss_n_05)
			
			// calculate solution of third step and save it in process
			*pq =  save_qn   + h * dq_n_s3;
			*pdq = save_dqn  + h * ddq_n_s3;
			parentSim->lProcess->myEvaluationExplicit(time+h);	// calculate new acceleration for next step at t = t_n + h

// **********************************	   
// fourth step	

			lmx::Vector<double> dq_n_s4(save_qn.size());	//used
			lmx::Vector<double> ddq_n_s4(save_qn.size());	//used


			// save f(t_n_1,q_s_n_1)			
			dq_n_s4     = *pdq; 	
			ddq_n_s4    = *pddq;


			// calculate next step
			// q_n_1 = q_n + h/6 * (f(t_n,q_n) + f(t_n_05,q_s_n_05) + f(t_n_1,q_ss_n_05) +  f(t_n_1,q_s_n_1))

			*pq = 	save_qn + h/6 *	(dq_n_s1 +  dq_n_s2 * 2 + dq_n_s3 * 2+ dq_n_s4);
			*pdq = save_dqn + h/6 *(ddq_n_s1 +  ddq_n_s2 * 2+  ddq_n_s3 * 2 + ddq_n_s4);	
	
			parentSim->lProcess->myEvaluationExplicit(time+h);	// calculate new acceleration for next step at t = t_n + h		
*/
}

void Integrator::cd(double time, int timeFlag)
{
	// (*pq) es el vector posicion en n
	// (*pdq) es el vector velocidad en n
	
	int i;
	if (timeFlag==1)
	{
		dqHalf.resize(pq->size());
		for (i=0; i<(*pq).size(); ++i) 
		{
			dqHalf[i] = (*pdq)[i] + h/2 * (*pddq)[i]; // new velocity at n+1/2
		}
	}
	else 
	{
		for (i=0; i<(*pq).size(); ++i) 
		{
			dqHalf[i] = dqHalf[i] + h * (*pddq)[i]; // new velocity at n+1/2
		}
	}
	/*
	cout << "Vel Half = " << endl;
	printSTLVector(dqHalf);
	*/
	for (i=0; i<(*pq).size(); ++i) 
	{
		(*pq)[i]  = (*pq)[i]  + h * dqHalf[i];	 // new position at n+1
		(*pdq)[i] =dqHalf[i] + h/2 * (*pddq)[i]; // new velocity at n+1		
	}
	
	/*
	int i;
	if (timeFlag==1) 
	{
		dqHalf.resize(pq->size());
		for (i=0; i<(*pq).size(); ++i) 
		{
			dqHalf[i] = (*pdq)[i] + h/2 * (*pddq)[i]; // new velocity at n+1/2
			(*pq)[i]  = (*pq)[i]  + h * dqHalf[i];	 // new position at n+1
			(*pdq)[i] =dqHalf[i] + h/2 * (*pddq)[i]; // new velocity at n+1		
		}
	}
	else 
	{
		for (i=0; i<(*pq).size(); ++i) 
		{
			dqHalf[i] = dqHalf[i] + h * (*pddq)[i]; // new velocity at n+1/2
			(*pq)[i]  = (*pq)[i]  + h * dqHalf[i];	 // new position at n+1
			(*pdq)[i] =dqHalf[i] + h/2 * (*pddq)[i]; // new velocity at n+1			
		}
	}
	*/
	/*
	for (i=0; i<(*pq).size(); ++i) 
	{	
		if (timeFlag==1) 
		{
			dqHalf.resize(pq->size());
			dqHalf[i] = (*pdq)[i] + h/2 * (*pddq)[i]; // new velocity at n+1/2
			(*pq)[i]  = (*pq)[i]  + h * dqHalf[i];	 // new position at n+
			(*pdq)[i] =dqHalf[i] + h/2 * (*pddq)[i]; // new velocity at n+
			
		}
		else 
		{
			dqHalf[i] = dqaux[i] + h * (*pddq)[i]; // new velocity at n+1/2
			(*pq)[i]  = (*pq)[i]  + h * dqHalf[i];	 // new position at n+
			(*pdq)[i] =dqHalf[i] + h/2 * (*pddq)[i]; // new velocity at n+
		}
	}
	*/
	/*
	cout << "Vel12";
	printSTLVector(dqaux);
	cout << endl;
	cout << "Vel";
	printSTLVector((*pdq));
	cout << endl;
	cout << "Ac";
	printSTLVector((*pddq));
	cout << endl;
	*/
	
}

// ***

void Integrator:: ab_1(double time, int timeFlag)
{
/*
		parentSim->lProcess->myEvaluationExplicit(time); // check if necessary, no it is calculated two times

		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// update history 

			// velocity vectors
			dq_n = *pdq;
			// acceleration vectors
			ddq_n = *pddq;
		}
	

		*pq  = *pq  + h * *pdq;	 // new position at n+1
		*pdq = *pdq + h * *pddq; // new velocity at n+1
	
		parentSim->lProcess->myEvaluationExplicit(time+h); // new acceleration at at n+1
*/
}

// ***

void Integrator:: ab_2(double time, int timeFlag)
{
/*
		parentSim->lProcess->myEvaluationExplicit(time); // check if necessary, no it is calculated two times

		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// update history 

			// velocity vectors
			dq_n1 = dq_n;
			dq_n = *pdq;

			// acceleration vectors
			ddq_n1 = ddq_n;
			ddq_n = *pddq;
	
		}

		*pq = *pq + h/2.0 *(3.0 * dq_n - 1.0 * dq_n1);	// calculate velocity
		*pdq = *pdq + h/2.0 *(3.0 * ddq_n - 1.0 * ddq_n1);	// calculate acceleration

		parentSim->lProcess->myEvaluationExplicit(time+h); // new acceleration at at n+1 // check if necessary, no it is calculated two times	
*/
}

// ***

void Integrator:: ab_3(double time, int timeFlag)
{
/*
		parentSim->lProcess->myEvaluationExplicit(time); // check if necessary, no it is calculated two times

		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// update history 

			// velocity vectors
			dq_n2 = dq_n1;
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n2 = ddq_n1;
			ddq_n1 = ddq_n;
			ddq_n = *pddq;

		}

		*pq = *pq + h/12.0 *(23.0 * dq_n - 16.0 * dq_n1 + 5.0 * dq_n2);	// calculate velocity
		*pdq = *pdq + h/12.0 *(23.0 * ddq_n - 16.0 * ddq_n1 + 5.0 * ddq_n2);	// calculate acceleration

		parentSim->lProcess->myEvaluationExplicit(time+h); // new acceleration at at n+1 // check if necessary, no it is calculated two times		
*/
}

// ***

void Integrator:: ab_4(double time, int timeFlag)
{	
/*
		parentSim->lProcess->myEvaluationExplicit(time); // check if necessary, no it is calculated two times

		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// update history 

			// velocity vectors
			dq_n3 = dq_n2;
			dq_n2 = dq_n1;
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n3 = ddq_n2;
			ddq_n2 = ddq_n1;
			ddq_n1 = ddq_n;
			ddq_n = *pddq;

		}

		*pq = *pq + h/24.0 *(55.0 * dq_n - 59.0 * dq_n1 + 37.0 * dq_n2 - 9.0 * dq_n3);	// calculate velocity
		*pdq = *pdq + h/24.0 *(55.0 * ddq_n - 59.0 * ddq_n1 + 37.0 * ddq_n2 - 9.0 * ddq_n3);	// calculate acceleration

		parentSim->lProcess->myEvaluationExplicit(time+h); // new acceleration at at n+1 // check if necessary, no it is calculated two times			
*/
}

// ***

void Integrator:: ab_5(double time, int timeFlag)
{

	// to be done	
		
}

// ***

void Integrator:: am_1(double time, int timeFlag)	
{
/*
		if(timeFlag) // check if new calculation of the constant part is necessary
		{

			// position vectors
			q_n = *pq;

			// velocity vectors
			dq_n = *pdq;

			// acceleration vectors
			ddq_n = *pddq;		


			//dq_n_1_const =  dq_n;		// calculate constant part 
			//ddq_n_1_const = dqd_n;	// calculate constant part
		}


		
		*pdq =  1.0 / h * (*pq - q_n) ;	// calculate velocity
		*pddq = 1.0 / h * (*pdq - dq_n) ;	// calculate acceleration
		cout << "test\n";
*/
}

		

// ***

void Integrator:: am_2(double time, int timeFlag)
{	
/*
		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// position vectors
			q_n = *pq;

			// velocity vectors
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n1 = ddq_n;
			ddq_n = *pddq;		


			dq_n_1_const =  0.5 * dq_n;		// calculate constant part 
			ddq_n_1_const = 0.5 * ddq_n;	// calculate constant part
	
		}

		*pdq = (1.0/h*(*pq - q_n) -  dq_n_1_const) * 2.0;	// calculate velocity
		*pddq = (1.0/h*(*pdq - dq_n) -  ddq_n_1_const) * 2.0;	// calculate acceleration
*/		
	
}

// ***

void Integrator:: am_3(double time, int timeFlag)
{	
/*
		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// position vectors
			q_n = *pq;

			// velocity vectors
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n1 = ddq_n;
			ddq_n = *pddq;		


			dq_n_1_const =  8.0/12.0 * dq_n - 1.0/12.0 * dq_n1;		// calculate constant part 
			ddq_n_1_const = 8.0/12.0 * ddq_n - 1.0/12.0 * ddq_n1;	// calculate constant part
	
		}

		*pdq = (12.0/5.0) * (1.0 / h * (*pq - q_n) -  dq_n_1_const);	// calculate velocity
		*pddq = (12.0/5.0) * (1.0 / h * (*pdq - dq_n) -  ddq_n_1_const);	// calculate acceleration
*/
}

// ***

void Integrator:: am_4(double time, int timeFlag)
{	
/*
		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// position vectors
			q_n = *pq;

			// velocity vectors
			dq_n2 = dq_n1;
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n2 = ddq_n1;
			ddq_n1 = ddq_n;
			ddq_n = *pddq;		


			dq_n_1_const =  19.0/24.0 * dq_n - 5.0/24.0 * dq_n1 + 1.0/24.0 * dq_n2;		// calculate constant part 
			ddq_n_1_const = 19.0/24.0 * ddq_n - 5.0/24.0 * ddq_n1 + 1.0/24.0 * ddq_n2;		// calculate constant part
	
		}

		*pdq = (24.0/9.0) * (1.0 / h * (*pq - q_n) -  dq_n_1_const);	// calculate velocity
		*pddq = (24.0/9.0) * (1.0 / h * (*pdq - dq_n) -  ddq_n_1_const);	// calculate acceleration
*/
}

// ***

void Integrator:: am_5(double time, int timeFlag)
{	
/*
		if(timeFlag) // check if new calculation of the constant part is necessary
		{
			// position vectors
			q_n = *pq;

			// velocity vectors
			dq_n3 = dq_n2;
			dq_n2 = dq_n1;
			dq_n1 = dq_n;
			dq_n = *pdq;


			// acceleration vectors
			ddq_n3 = ddq_n2;
			ddq_n2 = ddq_n1;
			ddq_n1 = ddq_n;
			ddq_n = *pddq;		


			dq_n_1_const =  646.0/720.0 * dq_n - 264.0/720.0 * dq_n1 + 106.0/720.0 * dq_n2 - 19.0/720.0 * dq_n3;			// calculate constant part 
			ddq_n_1_const = 646.0/720.0 * ddq_n - 264.0/720.0 * ddq_n1 + 106.0/720.0 * ddq_n2 - 19.0/720.0 * ddq_n3;	// calculate constant part
	
		}

		*pdq = (720.0/251.0) * (1.0 / h * (*pq - q_n) -  dq_n_1_const);		// calculate velocity
		*pddq = (720.0/251.0) * (1.0 / h * (*pdq - dq_n) -  ddq_n_1_const);	// calculate acceleration
*/
}

// ***

// *******************************************************************************************************************
//
// 
// ***

void Integrator::setPartials(double h, string tp)
{

	if (!tp.compare("alphagen" ))
	{
		partial_dq  = gamma / (h * beta);
		partial_ddq = 1.0/ (h * h * beta) * (1.0 - alpha_m)/(1.0 - alpha_f);
	}					
	else if  (!tp.compare("newmark"))
	{
		partial_dq = gamma / (h * beta);
		partial_ddq = 1.0/ (h * h * beta);
	}
	else if (!tp.compare("midpoint"))
	{
//		partial_dq = 1.0 / h;	// WARNING: This is the derivative of the midpoint velocity respect to n+1 position
		partial_dq = 2.0 / h;	// WARNING: This is the derivative of the n+1 velocity respect to n+1 position		
		partial_ddq = 2.0 / (h * h);
	}	
	else if (!tp.compare("consistent"))
    {
        partial_dq = 2.0 / h;   // WARNING: This is the derivative of the n+1 velocity respect to n+1 position
        partial_ddq = 2.0 / (h * h);
    }
	else if (!tp.compare("ab-1" ))
	{
		// to be done
	}
	else if (!tp.compare("bdf-1" ))
	{
		partial_dq = 1.0 / (h * 1.0); 
		partial_ddq = 1.0 / ((h * 1.0) * (h * 1.0)); 
	}
	else if (!tp.compare("bdf-2" ))
	{
		partial_dq = 1 / (h * 2.0/3.0);
		partial_ddq = 1 / ((h * 2.0/3.0) * (h * 2.0/3.0)); 
	}
	else if (!tp.compare("bdf-3" ))
	{
		partial_dq = 1 / (h * 6.0/11.0); 
		partial_ddq = 1 / ((h * 6.0/11.0) * (h * 6.0/11.0)); 
	}
	else if (!tp.compare("bdf-4" ))
	{
		partial_dq = 1 / (h * 12.0/25.0); 
		partial_ddq = 1 / ((h * 12.0/25.0) * (h * 12.0/25.0)); 
	}
	else if (!tp.compare("bdf-5" ))
	{
		partial_dq = 1 / (h * 60.0/137.0); 
		partial_ddq = 1 / ((h * 60.0/137.0) * (h * 60.0/137.0)); 
	}
	else if (!tp.compare("rk-1"))
	{
	}
	else if (!tp.compare("rk-2"))
	{
	}
	else if (!tp.compare("rk-3"))
	{
	}
	else if (!tp.compare("rk-4"))
	{
	}
	else if (!tp.compare("am-1"))
	{
		partial_dq = 1.0 / (h * 1.0); 
		partial_ddq = 1.0 / ((h * 1.0) * (h * 1.0)); 	
	}
	else if (!tp.compare("am-2"))
	{
		partial_dq = 1.0 / (h * 0.5); 
		partial_ddq = 1.0 / ((h * 0.5) * (h * 0.5)); 	
	}
	else if (!tp.compare("am-3"))
	{
		partial_dq = 1.0 / (h * 5.0/12.0); 
		partial_ddq = 1.0 / ((h * 5.0/12.0) * (h * 5.0/12.0)); 	
	}
	else if (!tp.compare("am-4"))
	{
		partial_dq = 1.0 / (h * 9.0/24.0); 
		partial_ddq = 1.0 / ((h * 9.0/24.0) * (h * 9.0/24.0)); 	
	}
	else if (!tp.compare("am-5"))
	{
		partial_dq = 1.0 / (h * 251.0/720.0); 
		partial_ddq = 1.0 / ((h * 251.0/720.0) * (h * 251.0/720.0)); 	
	}
}


// ***

double Integrator::getPartial_dq()
{
	return partial_dq;
}

// ***

double Integrator::getPartial_ddq()
{
	return partial_ddq;
}


